#include "ros/ros.h"

#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "nav_msgs/Path.h"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>

#include "stdio.h"
#include "unistd.h"
#include "../prob_model/odom_model.h"
#include "../prob_model/sensor_model.h"

#define LOGFILE "data/log/robotdata1.log"
#define MAX_BEAMS 180

inline double L2(double x1, double y1, double x2, double y2)
{
	return ((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
}

class PF
{
private:
	ros::NodeHandle n;
	ros::NodeHandle pn;
	FILE* fd;
	int seed;

	boost::mt19937 gen; // for all the random number staff

	OdomModel odom_model;
	SensorModel  sens_model;

	ros::Publisher pose_pub;

	geometry_msgs::PoseArray particles;

//	boost::uniform_real<> uniform;

	bool update;
	int max_particle;
	double wait;
	double time_stamp;
	double l2_limit, theta_limit;
	double w_tot, w_avg, w_slow, w_fast, alpha_slow, alpha_fast;
	double resample_rate;

//	double prev_x, prev_y, prev_yaw, next_x, next_y, next_yaw;
	std::vector<int> laser; // beams are in cm, will be converted to m in sensor node
	std::vector<double> weights;
	boost::variate_generator<boost::mt19937&, boost::uniform_real<> > uniform_0to1;

public:
	PF(ros::NodeHandle& p, int sd)
	:n(),pn(p),fd(NULL),seed(sd), gen(seed),
	 odom_model(n, pn, gen),sens_model(n, pn, gen),update(false),
	 w_slow(0.0),w_fast(0.0),laser(MAX_BEAMS, 0),
	 uniform_0to1(gen, boost::uniform_real<> (0.0, 1.0))
	{
		std::string logfile, global_frame_id;
		pn.param("logfile", logfile, std::string(LOGFILE));
		pn.param("frame_id", global_frame_id, std::string("map"));
		pn.param("max_p", max_particle, 50000);
		pn.param("wait_time", wait, 1e6);
		pn.param("l2_limit", l2_limit, 1e-4);
		pn.param("theta_limit", theta_limit, 0.02);
		pn.param("alpha_slow", alpha_slow, 6.0);
		pn.param("alpha_fast", alpha_fast, 6.0);
		pn.param("resample_rate", resample_rate, 0.0);

		ROS_INFO("Open log file at: %s", logfile.c_str());
		ROS_INFO("Maximum particles %d", max_particle);
		ROS_INFO("af = %.4lf as = %.4lf", alpha_fast, alpha_slow);
		ROS_WARN("The seed was %d", seed);

		weights.resize(max_particle, 1.0);

		// Open up the log file
		fd = fopen(logfile.c_str(), "r");
		if (fd == 0)
		{
			ROS_ERROR("Couldn't load log file");
			exit(1);
		}


		pose_pub = n.advertise<geometry_msgs::PoseArray>("particles", 2);

		particles.header.stamp = ros::Time::now();
		particles.header.frame_id = global_frame_id;
	}

	~PF(){}

	void main_loop()
	{
		// spread the particles
		sens_model.addNRandParticles(max_particle, particles.poses);
		usleep(wait);
		pose_pub.publish(particles);
		ROS_INFO("First publish");

		char ch;
		assert(fscanf(fd, " %c", &ch) == 1);
		while(ch !=EOF && ros::ok())
		{
			switch (ch)
			{
			case 'L':
				updateMeasurement();
				augmentedResample();
				break;
			case 'O':
				updateOdometry();
				break;
			case EOF:
			default:
				break;
			}
			usleep(wait);
			// publish poseArray
			pose_pub.publish(particles);
			if(1 != fscanf(fd, " %c", &ch))
				break;
		}
		ROS_WARN("The seed was %d", seed);
		ROS_WARN("This is the end of main loop");
	}

	void updateOdometry()
	{
		static bool not_1st = false;
		static double prev_x, prev_y, prev_yaw;
		double next_x, next_y, next_yaw;

		int status = fscanf(fd, "%lf%lf%lf%lf", &next_x, &next_y, &next_yaw, &time_stamp);
		assert(status == 4);

		if (not_1st)
		{
			double l2 = L2(next_x, next_y, prev_x, prev_y);
			double ad = fabs(angle_diff(prev_yaw, next_yaw));
			if ( l2 < l2_limit && ad < theta_limit)
			{
				ROS_WARN("Sensor update at: %.6lf.(l2=%.4lf < %.4lf, ad=%.4lf < %.4lf)", time_stamp, l2, l2_limit, ad, theta_limit);
				return;
			}
			else
			{
				ROS_INFO("Motion update at: %.6lf", time_stamp);
				odom_model.updateMotion(particles, prev_x, prev_y, prev_yaw, next_x, next_y, next_yaw);
			}
		}
		else
		{
			ROS_INFO("First motion update");
			not_1st = true;
		}

		update = true;
		prev_x = next_x;
		prev_y = next_y;
		prev_yaw = next_yaw;
	}

	void updateMeasurement()
	{
//		static bool is_1st = true;
		static double prev_x, prev_y, prev_yaw;
		double next_x, next_y, next_yaw;
		double sx,sy,st;
		int status = fscanf(fd, "%lf%lf%lf%lf%lf%lf", &next_x, &next_y, &next_yaw,&sx,&sy,&st);
		assert(status == 6);

		// Here read in all the data
		for(int i=0; i< MAX_BEAMS; i++)
		{
			if(fscanf(fd, " %d", &laser[i]) != 1)
				ROS_ERROR("Error reading in laser data at %d, value %d", i, laser[i]);
		}
		assert(1 ==fscanf(fd, " %lf", &time_stamp) );

		if (!update)
		{
			return;
		}

		double l2 = L2(next_x, next_y, prev_x, prev_y);
		double ad = fabs(angle_diff(prev_yaw, next_yaw));
		if ( l2 < l2_limit && ad < theta_limit)
		{
			ROS_WARN("Sensor update at: %.6lf. (l2=%.5lf < %.5lf, ad=%.5lf < %.5lf)", time_stamp, l2, l2_limit, ad, theta_limit);
			return;
		}

		// reset total_weights and average weight
		w_tot = 0.0;
		w_avg = 0.0;
		int max_idx =0;
		double max_conf =0.0;
		for(size_t i=0; i< weights.size(); i++)
		{
			weights[i] = sens_model.getWeight(particles.poses[i], laser);
			w_tot += weights[i];
			if( max_conf < weights[i])
			{
				max_conf = weights[i];
				max_idx = i;
			}

		}
		w_avg =w_tot / weights.size();

		sens_model.publish_bestMeasurement(particles.poses[max_idx], laser);

		prev_x = next_x;
		prev_y = next_y;
		prev_yaw = next_yaw;
	}

	void augmentedResample()
	{
		if (!update)
		{
			return;
		}

		update = false;
		if (w_slow <= 0.0)
			w_slow = w_avg;
		else
			w_slow += alpha_slow*(w_avg - w_slow);

		if (w_fast <= 0.0)
			w_fast = w_avg;
		else
			w_fast += alpha_fast*(w_avg - w_fast);

		// normalize weight:
		for(size_t i=0; i< weights.size(); i++)
			weights[i] /= w_tot;

		low_variance_resample(); // resample the particles according to their weights
	}


protected: // just to separate functions and variables
/*
	void inject_rand_samples()// for motion update
	{
		int total_samples = particles.poses.size();
		static boost::uniform_int<> rng(0, total_samples);
		int m = rng(gen);// * total_samples);
		int n = total_samples * rs_ratio;
		int interval = (1.0 / rs_ratio);
//		ROS_INFO("Inject %d new rand samples", n);
//		ROS_INFO("m %d n %d", m, n);
		for (int i=0 ; i< n; ++i)
		{
			m = (m+interval) % total_samples;
//			ROS_INFO("m %d", m);
			particles.poses[m] = sens_model.getOneRandParticle();
		}
	}*/

	void low_variance_resample()
	{
		double interval = 1.0/max_particle;
		double resampleTH = 1.0 - w_fast/w_slow;
		if (resampleTH <= resample_rate)
		{
			resampleTH = resample_rate;
			ROS_INFO("Sensor update at: %.6lf, rs %.8lf ", time_stamp, resampleTH);
		}
		else
		{
			ROS_WARN("Sensor update at: %.6lf, rs %.8lf ", time_stamp, resampleTH);
		}

		std::vector<geometry_msgs::Pose> temp;

		double r = uniform_0to1() * interval; // a random number [0, 1/M];
		double c = weights[0];
		int i=0;
		for(int m=0; m< max_particle; ++m)
		{
			double U = r+  m * interval;
			while (U > c)
			{
				i++;
				c += weights[i];
			}
			temp.push_back(particles.poses[i]);
		}

		static boost::uniform_int<> rng(0, max_particle);
		if (resampleTH > 0.0)
		{
			int m = rng(gen);// * total_samples);
			int n = max_particle * resampleTH;
			int interval = 1.0 / resampleTH;
			for(int j=0; j< n; ++j)
			{
				m = (m+interval) % max_particle;
				temp[m] =sens_model.getOneRandParticle();
			}
		}
		particles.poses.resize(0);
		particles.poses = temp;
	}
};

int main(int argc, char* argv[])
{
	ros::init(argc, argv, "particle_filter");

	ros::NodeHandle p("~");
	int seed;
	p.param("seed", seed, 0);
	if (seed == 0)
		seed = time(0);

	PF particle_filter(p, seed);

	particle_filter.main_loop();

	//	particle_filter.debug_loop();

	return 0;
}

/*
 *
 * 	void debug_loop()
	{
		while(ros::ok())
		{
			particles.poses.resize(0);
			sens_model.addNRandParticles(max_particle, particles.poses);
			pose_pub.publish(particles);
			usleep(1e5);
		}
	}



	void fakeupdateMeasurement()
		{
			static double prev_x, prev_y, prev_yaw;
			double next_x, next_y, next_yaw;

			double time_stamp;
			double sx,sy,st;
			int status = fscanf(fd, "%lf%lf%lf%lf%lf%lf", &next_x, &next_y, &next_yaw,&sx,&sy,&st);
			assert(status == 6);

			for(int i=0; i< MAX_BEAMS; i++)
			{
				if(fscanf(fd, " %d", &laser[i]) != 1)
					ROS_ERROR("Error reading in laser data at %d, value %d", i, laser[i]);
			}

			fscanf(fd, " %lf", &time_stamp);

			std::vector<double> weights(particles.poses.size(), 1.0);

			weights = sens_model.getAllWeight(particles.poses, laser);

			ROS_INFO("Sensor update at: %.4lf, max conf %.8lf: ", time_stamp, 1.0);

			prev_x = next_x;
			prev_y = next_y;
			prev_yaw = next_yaw;
		}

 */
